import math

import numpy as np


class Robot:
    def __init__(self, position, goal, radius):
        self.position = position
        self.goal = goal
        self.radius = radius
        self.speed = 0
        self.direction = 0

    def update(self, delta_t, speed, angle):
        # 更新机器人位置和方向
        self.speed = speed
        self.direction += angle
        x = self.position[0] + speed * delta_t * math.cos(self.direction)
        y = self.position[1] + speed * delta_t * math.sin(self.direction)
        self.position = (x, y)

def check_collision(robot1, robot2):
    distance = math.sqrt((robot1.position[0] - robot2.position[0])**2 + (robot1.position[1] - robot2.position[1])**2)
    return distance < robot1.radius + robot2.radius

def avoid_collision(robot, other_robot):
    """
    避免机器人之间的碰撞。
    """
    # 计算机器人和其他机器人的距离和重叠距离
    distance = math.sqrt((robot.position[0] - other_robot.position[0])**2 +
                         (robot.position[1] - other_robot.position[1])**2)
    overlap = robot.radius + other_robot.radius - distance

    # 计算机器人的速度向量
    velocity = calculate_velocity(robot, [other_robot], delta_t)[0]

    # 根据距离和重叠距离调整机器人的位置
    if overlap > 0:
        # 计算需要调整的位置
        adjust_x = velocity[0] / math.sqrt(velocity[0]**2 + velocity[1]**2) * overlap / 2
        adjust_y = velocity[1] / math.sqrt(velocity[0]**2 + velocity[1]**2) * overlap / 2

        # 更新机器人的位置
        robot.position = (robot.position[0] + adjust_x, robot.position[1] + adjust_y)
        other_robot.position = (other_robot.position[0] - adjust_x, other_robot.position[1] - adjust_y)

def check_path(robot, other_robot):
    """
    检查机器人之间的路径是否有重叠。
    """
    # 获取机器人的当前位置和目标位置
    start_position = robot.position
    end_position = robot.goal

    # 获取其他机器人的当前位置和目标位置
    other_start_position = other_robot.position
    other_end_position = other_robot.goal

    # 检查机器人之间的路径是否有重叠
    path1 = LineSegment(np.array(start_position), np.array(end_position))
    path2 = LineSegment(np.array(other_start_position), np.array(other_end_position))
    intersection = path1.intersects(path2)

    return intersection

def adjust_path(robot, other_robot):
    """
    对机器人的路径进行规划，以避免与其他机器人发生碰撞。
    """
    # 获取机器人的当前位置和目标位置
    start_position = robot.position
    end_position = robot.goal

    # 获取其他机器人的当前位置和目标位置
    other_start_position = other_robot.position
    other_end_position = other_robot.goal

    # 计算机器人和其他机器人的速度向量
    velocity = calculate_velocity(robot, [other_robot], delta_t)[0]
    other_velocity = calculate_velocity(other_robot, [robot], delta_t)[0]

    # 计算机器人和其他机器人的运动方向
    direction = math.atan2(velocity[1], velocity[0])
    other_direction = math.atan2(other_velocity[1], other_velocity[0])

    # 计算机器人和其他机器人的速度大小
    speed = math.sqrt(velocity[0]**2 + velocity[1]**2)
    other_speed = math.sqrt(other_velocity[0]**2 + other_velocity[1]**2)

    # 计算机器人和其他机器人之间的距离和重叠距离
    distance = math.sqrt((start_position[0] - other_start_position[0])**2 +
                         (start_position[1] - other_start_position[1])**2)
    overlap = robot.radius + other_robot.radius - distance

    # 计算需要调整的距离
    adjust_distance = overlap / 2

    # 根据需要调整的距离和速度调整机器人的路径
    if speed == 0 or other_speed == 0:
        return False
    time_to_collision = adjust_distance / (speed + other_speed)
    if time_to_collision < delta_t:
        # 机器人需要避让
        new_direction = direction + math.pi / 2
        new_position = (start_position[0] + adjust_distance * math.cos(new_direction),
                        start_position[1] + adjust_distance * math.sin(new_direction))
        robot.goal = new_position
        return True
    else:
        return False

class LineSegment:
    def __init__(self, start, end):
        self.start = start
        self.end = end

    def intersects(self, other):
        # 计算两条线段的四个端点
        print(self.start)
        p1, q1 = self.start, self.end
        p2, q2 = other.start, other.end

        # 计算两条线段的方向向量和垂直向量
        v1, v2 = q1 - p1, q2 - p2
        u1, u2 = np.array([-v1[1], v1[0]]), np.array([-v2[1], v2[0]])

        # 计算两条线段在垂直方向上的投影长度
        d1 = np.dot(u1, p2 - p1)
        d2 = np.dot(u1, q2 - p1)
        d3 = np.dot(u2, p1 - p2)
        d4 = np.dot(u2, q1 - p2)

        # 如果两条线段的投影有重叠，则说明它们相交
        if d1 * d2 < 0 and d3 * d4 < 0:
            return True
        elif d1 == 0 and self._on_segment(p2):
            return True
        elif d2 == 0 and self._on_segment(q2):
            return True
        elif d3 == 0 and other._on_segment(p1):
            return True
        elif d4 == 0 and other._on_segment(q1):
            return True
        else:
            return False

    def _on_segment(self, point):
        # 检查一个点是否在当前线段上
        return (min(self.start[0], self.end[0]) <= point[0] <= max(self.start[0], self.end[0])
                and min(self.start[1], self.end[1]) <= point[1] <= max(self.start[1], self.end[1]))


def calculate_velocity(robot, robots, delta_t):
    # 计算机器人的速度和方向，避免碰撞
    max_speed = 6.0
    pref_speed = max_speed
    radius = robot.radius
    position = robot.position
    goal = robot.goal
    velocity = [0.0, 0.0]

    for other_robot in robots:
        if robot != other_robot:
            if check_collision(robot, other_robot):
                # 计算速度障碍
                relative_pos = (other_robot.position[0] - position[0], other_robot.position[1] - position[1])
                dist = math.sqrt(relative_pos[0] ** 2 + relative_pos[1] ** 2)
                if dist < 2 * radius:
                    # 机器人之间距离太近，需要避免碰撞
                    new_velocity = [0.0, 0.0]
                    new_velocity[0] = (relative_pos[0] / dist) * ((2 * radius) / delta_t - pref_speed)
                    new_velocity[1] = (relative_pos[1] / dist) * ((2 * radius) / delta_t - pref_speed)
                    if abs(new_velocity[0]) > abs(velocity[0]):
                        velocity[0] = new_velocity[0]
                    if abs(new_velocity[1]) > abs(velocity[1]):
                        velocity[1] = new_velocity[1]

    # 计算朝向目标位置的速度
    relative_goal = (goal[0] - position[0], goal[1] - position[1])
    dist = math.sqrt(relative_goal[0] ** 2 + relative_goal[1] ** 2)
    if dist > 0:
        pref_velocity = (relative_goal[0] / dist * pref_speed, relative_goal[1] / dist * pref_speed)
        if abs(pref_velocity[0]) > abs(velocity[0]):
            velocity[0] = pref_velocity[0]
        if abs(pref_velocity[1]) > abs(velocity[1]):
            velocity[1] = pref_velocity[1]

    # 返回速度向量
    return velocity


# 初始化机器人
robot1 = Robot((0, 0), (10, 10), 0.5)
robot2 = Robot((0, 5), (10, 5), 0.5)
robot3 = Robot((5, 0), (5, 10), 0.5)
robot4 = Robot((5, 5), (5, 5), 0.5)
robots = [robot1, robot2, robot3, robot4]

# 主循环
delta_t = 0.02  # 时间步长，单位为秒
while True:
    # 计算每个机器人的速度向量
    velocities = []
    for robot in robots:
        velocity = calculate_velocity(robot, robots, delta_t)
        velocities.append(velocity)

    # 更新机器人的位置和方向
    for i, robot in enumerate(robots):
        # 计算机器人的角速度
        v = velocities[i]
        # angle = robot.angle
        angular_velocity = (v[1] / robot.radius - v[0] / robot.radius) / delta_t

        # 计算机器人的线速度
        linear_velocity = math.sqrt(v[0] ** 2 + v[1] ** 2)

        # 更新机器人的位置和方向
        new_x = robot.position[0] + v[0] * delta_t
        new_y = robot.position[1] + v[1] * delta_t
        # new_angle = angle + angular_velocity * delta_t

        robot.position = (new_x, new_y)
        # robot.angle = new_angle

    # 使用碰撞检测和路径规划算法避免碰撞
    for i, robot in enumerate(robots):
        for j, other_robot in enumerate(robots):
            if i < j:
                if check_collision(robot, other_robot):
                    avoid_collision(robot, other_robot)
                if check_path(robot, other_robot):
                    adjust_path(robot, other_robot)

